#ifndef AUTOAIM_MYUTILS__UTILS_HPP_
#define AUTOAIM_MYUTILS__UTILS_HPP_

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/opencv.hpp>
#include <vector>

#include "rm_interfaces/Armor.h"

namespace device_type {
typedef enum { NUC, NX, OTHERS } DEVICE;
}

namespace utils {

template <typename T, typename EigenT>
void toEigenMatrix(EigenT &matrix, const std::vector<T> &vector) {

  int cnt = 0;
  for (int row = 0; row < matrix.rows(); row++) {
    for (int col = 0; col < matrix.cols(); col++) {
      matrix(row, col) = vector[cnt];
      cnt++;
    }
  }
}

template <typename T>
void toCvMatrix(cv::Mat &matrix, const std::vector<T> &vector) {
  int cnt = 0;
  for (int row = 0; row < matrix.rows; row++) {
    for (int col = 0; col < matrix.cols; col++) {
      matrix.at<T>(row, col) = vector[cnt];
      cnt++;
    }
  }
}

/**
 * @brief 坐标变换类，有parent_frame和child_frame
 *
 */
class TransformTool {
public:
  /**
   * @param quaternion quaternion from parent_frame to child_frame
   * @param t pretranslate
   */
  TransformTool(const Eigen::Quaterniond &quaternion,
                const Eigen::Vector3d &t) {
    quat_ = quaternion;
    quat_inv_ = quat_.conjugate();
    t_ = t;
  }
  TransformTool(const Eigen::Isometry3d &isometry) {
    quat_ = isometry.rotation();
    quat_inv_ = quat_.conjugate();
    t_ = isometry.translation();
  }

  Eigen::Vector3d Parent2Child(const Eigen::Vector3d &pos) {
    return quat_ * (pos + t_);
  }

  Eigen::Vector3d Child2Parent(const Eigen::Vector3d &pos) {
    return quat_inv_ * pos - t_;
  }

private:
  Eigen::Vector3d t_;
  Eigen::Quaterniond quat_;
  Eigen::Quaterniond quat_inv_;
};

} // namespace utils
#endif // AUTOAIM_MYUTILS__UTILS_HPP_